#include "AlgoEvaluatorModule12HardRestriction.h"

class algo::AlgoEvaluatorModule12HardRestrictionPrivate
{
public:
};

static bool isInside(const algobase::Coord& point, const algobase::Polygon& polygon)
{
    if (polygon.size() < 3) {
        return false;
    }
    int crossings = 0;
    for (size_t i = 0; i < polygon.size(); ++i) {
        const auto& p1 = polygon[i];
        const auto& p2 = polygon[(i + 1) % polygon.size()];
        if (((p1[1] <= point[1] && point[1] < p2[1]) || (p2[1] <= point[1] && point[1] < p1[1])) &&
            (point[0] < (p2[0] - p1[0]) * (point[1] - p1[1]) / (p2[1] - p1[1]) + p1[0])) {
            crossings++;
        }
    }
    return (crossings % 2) != 0;
}

algo::AlgoEvaluatorModule12HardRestriction::AlgoEvaluatorModule12HardRestriction()
	: d(new AlgoEvaluatorModule12HardRestrictionPrivate)
{
}

algo::AlgoEvaluatorModule12HardRestriction::~AlgoEvaluatorModule12HardRestriction()
{
	delete d;
}

std::vector<algobase::EvaluateEnvirDataType> algo::AlgoEvaluatorModule12HardRestriction::getNeedType()
{
	return std::vector<algobase::EvaluateEnvirDataType>({ algobase::EEDT_Depth, algobase::EEDT_Sonar });
}

int algo::AlgoEvaluatorModule12HardRestriction::exec(
    const algobase::Path& path, 
    double& result, 
    algobase::EvaluateModuleCoords& resultCoords)
{
	// return algo::AlgoEvaluatorModuleBase::exec(path, result, resultCoords);
    result = 1.0;
    resultCoords.resize(path.size());

    const auto& polygons = getTaskConfig().mTypePolygons;
    const auto restricted_it = polygons.find(algobase::AT_Restricted);
    const auto obstacle_it = polygons.find(algobase::AT_Obstacle);
    auto db = getDB();

    for (size_t i = 0; i < path.size(); ++i)
    {
        resultCoords[i].Assign(path[i]);
        resultCoords[i].mResult = 1.0; // Assume the point is valid initially

        const auto& current_point = path[i];
        algobase::Coord point_2d = { current_point.mCoordZ[0], current_point.mCoordZ[1] };

        // 1. Check against restricted and obstacle areas
        if (restricted_it != polygons.end()) {
            for (const auto& polygon : restricted_it->second) {
                if (isInside(point_2d, polygon)) {
                    resultCoords[i].mResult = 0.0;
                    break;
                }
            }
        }
        if (resultCoords[i].mResult == 0.0) {
            result = 0.0;
            continue;
        }

        if (obstacle_it != polygons.end()) {
            for (const auto& polygon : obstacle_it->second) {
                if (isInside(point_2d, polygon)) {
                    resultCoords[i].mResult = 0.0;
                    break;
                }
            }
        }
        if (resultCoords[i].mResult == 0.0) {
            result = 0.0;
            continue;
        }

        // 2. Check for terrain collision
        if (db) {
            // 2a. Check for collision with sea surface (depth > 0)
            if (current_point.mCoordZ[2] > 0) {
                resultCoords[i].mResult = 0.0;
            }
            else {
                // 2b. Check for collision with sea floor
                float terrain_depth = db->getData(algobase::EEDT_Depth, getTaskConfig().mStartTime, current_point.mCoordZ, 9999.0f); // Use an unlikely default value
                // Only check for collision if we got valid terrain data (not the default value)
                if (terrain_depth != 9999.0f && current_point.mCoordZ[2] < terrain_depth) {
                    resultCoords[i].mResult = 0.0;
                }
            }
        }

        if (resultCoords[i].mResult == 0.0) {
            result = 0.0;
        }
    }

    // The final result is the product of all points' results
    for (const auto& coord : resultCoords) {
        result *= coord.mResult;
    }

    return 0;
}

double algo::AlgoEvaluatorModule12HardRestriction::calcScore(double rawValue) const
{
    return 0.0;
}

double algo::AlgoEvaluatorModule12HardRestriction::calcRaw(const std::vector<double>& rawValues) const
{
    return 0.0;
}
